/*

	BLAH BLAH BLAH

*/

#define _CRTDBG_MAP_ALLOC
#include <stdlib.h>
#include <crtdbg.h>


#include <stdio.h>		// Simple I/O
#include <math.h>		// Simple math Routines
#include <windows.h>	// Header for Windows
#include <vector>
#include <algorithm>
#include <functional>

#include "all.h"
#include "drawer/gldrawer.h"
#include "drawer/camera.h"
#include "math/m_Quaternion.h"
#include "coldet/gjk.h"
#include "coldet/simplealgs.h"
#include "helpers/globalallocator.h"
#include "dynamics/joints.h"
#include "dynamics/solver_base.h"
#include "dynamics/solver_pgs.h"
#include "dynamics/solver_cg.h"
#include "dynamics/solver_lcpcg.h"
#include "FEM/FEM.h"
#include "FEM/joints.h"

bool UpdateFEM = false;
bool gbFreeze = false;


// Defines demo-scene (0 - ball-sockets, 1 - FEM)
#define DEMO_FEMCUBE 1
// Is FEM-Object in test scene fixed by the left side
#define DEMOCUBE_FIXED 0
// Is Ball-Sockets chain attached to FEM-Object
#define DEMO_FEMCUBE_CHAIN 1
// Is Plane constraint acting on FEM-Object
#define DEMO_FEMCUBE_PLANE 1
// Colorizes FE-strain if enables, otherwise - lambda vector magnitude
#define COLORING_STRAIN 0


class demoscene
{
public:

	std::vector<FEMJoint, GlobalAllocator<FEMJoint>> tetrajoints;
	std::vector<BallSocket, GlobalAllocator<BallSocket>> ballsockets;
	std::vector<BallSocket_FEM, GlobalAllocator<BallSocket_FEM>> ballsockets_fem;

	std::vector<PlaneConstraint, GlobalAllocator<PlaneConstraint>> planejoints;

	SolverPGS FEMTestSolver;

	// Key to determine which lambda should we render
	int LambdaRender;

	demoscene(): CubeSegWidth(8), CubeSegHeight(2), CubeSegDepth(1),
				 Time(0.0f), FEMTestSolver(15, 0.001f)
	{
	}

	void Init()
	{
		FEMTestSolver.SetInitialGuessCutFlag(true);

#if (DEMO_FEMCUBE == 1)

		LambdaRender = 0;

		FEMTestSolver.SetPrecision(0.0001f);
		FEMTestSolver.SetMaxIterations(30);

		CMatrix3 CubeRotation1;
		CubeRotation1.MakeIdentity();

		FEMCube.FEMNodes.push_back(Vec3(0.0f, 0.0f, 0.0f));
		BuildCube(FEMCube, Vec3(0.0f, 10.5f, 0.0f), Vec3(4.0f, 1.0f, 0.5f), CubeRotation1, CubeSegWidth, CubeSegHeight, CubeSegDepth);
		
		gLog.Print("Number of FE: %d;\n", FEMCube.FEMTetras.size());

		int size = FEMCube.FEMNodes.size();

		// based on density of rubber == 1100 kg / m^3
//		float TotalMass = 2200.0f;
		float TotalMass = 20.0f;

		//TODO: remember the offset for the cube in the solver
		for (int node = 1; node < size; ++node)
		{
			float mass = TotalMass / (float)size;

#if (DEMOCUBE_FIXED == 1)
			float invmass = node < 1 ? 0.0f : (1.0f / mass);
			if (node == 4 || node == 12) invmass = 0.0f;
#else
			float invmass = 1.0f / mass;
#endif

			unsigned idx = FEMTestSolver.AddTranslationalNode(invmass, FEMCube.FEMNodes[node], Vec3(0.0f, 0.0f, 0.0f));
		}

		tetrajoints.resize(FEMCube.FEMTetras.size());
		
		std::vector<FEMTetrahedron, GlobalAllocator<FEMTetrahedron>>::iterator itTetraEnd = FEMCube.FEMTetras.end(), itTetra;
		std::vector<FEMJoint, GlobalAllocator<FEMJoint>>::iterator itJointEnd = tetrajoints.end(), itJoint;

#if (DEMO_FEMCUBE_PLANE == 1)

		planejoints.resize(size);

		std::vector<PlaneConstraint, GlobalAllocator<PlaneConstraint>>::iterator itPlaneEnd = planejoints.end(), itPlane;

		Vec3 PlaneNrm(0, 1, 0), PlanePnt(0, -3, 0);
		unsigned Node_cnt = 0;

		for (itPlane = planejoints.begin(); itPlane != itPlaneEnd; ++itPlane)
		{
			itPlane->Init(FEMTestSolver, 0.08f, 0.0f, PlanePnt, PlaneNrm, Node_cnt++, 0.01f);
		}

#endif

		// Elasticity
		float Young_Modulus = 1000.0f * 1.0f * 1.0f * 1.0f * 1.0f;
		float Poisson_Ratio = 0.35f;

		// Plasticity
		// c_creep [0..1/dt]
		// c_yeld - threshold for plastic strain
		// c_max - maximum plastic strain for FE
		float c_yeld = 0.05f, c_creep = 0.01f, c_max = 10.0f;
		float beta = 0.05f;										// Beta (C.O.)

		unsigned FE_cnt = 0;
		for (itTetra = FEMCube.FEMTetras.begin(), itJoint = tetrajoints.begin(); itTetra != itTetraEnd; ++itTetra, ++itJoint)
		{
			++FE_cnt;

			itJoint->Init(FEMTestSolver, FEMCube.FEMNodes, itTetra->ind1, itTetra->ind2, itTetra->ind3, itTetra->ind4,
							Young_Modulus, Poisson_Ratio, c_yeld, c_creep, c_max, beta, 0.0001f);
		}

#if (DEMO_FEMCUBE_CHAIN == 1)

		// CHAIN
		int StartNumNodes = FEMTestSolver.m_NumNodes;

		float mass = 2.0f;

		CMatrix3 InertiaMatrix, ZeroMatrix;
		InertiaMatrix.MakeIdentity();
		InertiaMatrix.mMatrix[0][0] = 1.0f / (0.1f * mass);
		InertiaMatrix.mMatrix[1][1] = 1.0f / (0.1f * mass);
		InertiaMatrix.mMatrix[2][2] = 1.0f / (0.1f * mass);

		ZeroMatrix.MakeZero();

		int i, NumLinks = 10;

		// Add rigid bodies
		// 1st
		for (i = 0; i < NumLinks + 1; ++i)
		{
			FEMTestSolver.AddTranslationalNode((i == 0 || i == NumLinks+1)?0.0f:(1.0f / mass), Vec3(25 - i*2.0f, 10.3f, -0.3f), NullVec3);
			FEMTestSolver.AddRotationalNode((i == 0 || i == NumLinks+1)?ZeroMatrix:InertiaMatrix, CQuaternion(1.0f, 0.0f, 0.0f, 0.0f), NullVec3);
		}

		BallSocket SimpleSocket;
		Vec3 Anchor;

		// 1st
		for (i = 0; i < NumLinks; ++i)
		{
			Anchor.x =	 (FEMTestSolver.m_NodePosRot_x[StartNumNodes + (i) * 2]
						+ FEMTestSolver.m_NodePosRot_x[StartNumNodes + (i + 1) * 2]) * 0.5f;
			Anchor.y =	 (FEMTestSolver.m_NodePosRot_y[StartNumNodes + (i) * 2]
						+ FEMTestSolver.m_NodePosRot_y[StartNumNodes + (i + 1) * 2]) * 0.5f;
			Anchor.z =	 (FEMTestSolver.m_NodePosRot_z[StartNumNodes + (i) * 2]
						+ FEMTestSolver.m_NodePosRot_z[StartNumNodes + (i + 1) * 2]) * 0.5f;

			SimpleSocket.Init(FEMTestSolver, 0.5f, 0.01f, Anchor, StartNumNodes + (i)*2, StartNumNodes + (i)*2+1, StartNumNodes + (i + 1)*2, StartNumNodes + (i + 1)*2+1);

			ballsockets.push_back(SimpleSocket);
		}

		int LastLinearTriple = FEMTestSolver.m_NumNodes - 2;

		int trIdx[3];
		trIdx[0] = 48;
		trIdx[1] = 50;
		trIdx[2] = 51;

		BallSocket_FEM SimpleFemSocket;

		Anchor.x = FEMTestSolver.m_NodePosRot_x[LastLinearTriple] - 0.5f;
		Anchor.y = FEMTestSolver.m_NodePosRot_y[LastLinearTriple];
		Anchor.z = FEMTestSolver.m_NodePosRot_z[LastLinearTriple];


		SimpleFemSocket.Init(FEMTestSolver, 0.5f, 0.01f, Anchor, LastLinearTriple, LastLinearTriple + 1, trIdx[0], trIdx[1], trIdx[2]);
		ballsockets_fem.push_back(SimpleFemSocket);

#endif

#else

		FEMTestSolver.SetPrecision(0.0001f);
		FEMTestSolver.SetMaxIterations(100);

		float mass = 0.1f;

		CMatrix3 InertiaMatrix, ZeroMatrix;
		InertiaMatrix.MakeIdentity();

		// r = 0.5; r*r = 0.25;
		// mr*r * (2/5) = 0.4 * mass * 0.25
		InertiaMatrix.mMatrix[0][0] = 1.0f / (0.1f * mass);
		InertiaMatrix.mMatrix[1][1] = 1.0f / (0.1f * mass);
		InertiaMatrix.mMatrix[2][2] = 1.0f / (0.1f * mass);

		ZeroMatrix.MakeZero();

		int i, NumLinks = 20;

		// Add rigid bodies
		// 1st
		for (i = 0; i < NumLinks + 1; ++i)
		{
			FEMTestSolver.AddTranslationalNode((i == 0 || i == NumLinks+1)?0.0f:(1.0f / mass), Vec3(-i*2.0f, 0.0f, 0.0f), NullVec3);
			FEMTestSolver.AddRotationalNode((i == 0 || i == NumLinks+1)?ZeroMatrix:InertiaMatrix, CQuaternion(1.0f, 0.0f, 0.0f, 0.0f), NullVec3);
		}



		// 2nd
		for (i = 0; i < NumLinks + 1; ++i)
		{
			FEMTestSolver.AddTranslationalNode((i == 0 || i == NumLinks+1)?0.0f:(1.0f / mass), Vec3(-i*2.0f, 0.0f, 3.0f), NullVec3);
			FEMTestSolver.AddRotationalNode((i == 0 || i == NumLinks+1)?ZeroMatrix:InertiaMatrix, CQuaternion(1.0f, 0.0f, 0.0f, 0.0f), NullVec3);
		}

		// 3rd
		for (i = 0; i < NumLinks + 1; ++i)
		{
			FEMTestSolver.AddTranslationalNode((i == 0 || i == NumLinks+1)?0.0f:(1.0f / mass), Vec3(-i*2.0f, 0.0f, -3.0f), NullVec3);
			FEMTestSolver.AddRotationalNode((i == 0 || i == NumLinks+1)?ZeroMatrix:InertiaMatrix, CQuaternion(1.0f, 0.0f, 0.0f, 0.0f), NullVec3);
		}




		BallSocket SimpleSocket;
		Vec3 Anchor;

		// 1st
		for (i = 0; i < NumLinks; ++i)
		{
			Anchor.x = (FEMTestSolver.m_NodePosRot_x[i*2] + FEMTestSolver.m_NodePosRot_x[(i+1)*2]) * 0.5f;
			Anchor.y = (FEMTestSolver.m_NodePosRot_y[i*2] + FEMTestSolver.m_NodePosRot_y[(i+1)*2]) * 0.5f;
			Anchor.z = (FEMTestSolver.m_NodePosRot_z[i*2] + FEMTestSolver.m_NodePosRot_z[(i+1)*2]) * 0.5f;

			SimpleSocket.Init(FEMTestSolver, 0.1f, 0.09f, Anchor, i*2, i*2+1, (i+1)*2, (i+1)*2+1);

			ballsockets.push_back(SimpleSocket);
		}




		// 2nd
		for (i = NumLinks+1; i < (NumLinks)*2; ++i)
		{
			Anchor.x = (FEMTestSolver.m_NodePosRot_x[i*2] + FEMTestSolver.m_NodePosRot_x[(i+1)*2]) * 0.5f;
			Anchor.y = (FEMTestSolver.m_NodePosRot_y[i*2] + FEMTestSolver.m_NodePosRot_y[(i+1)*2]) * 0.5f;
			Anchor.z = (FEMTestSolver.m_NodePosRot_z[i*2] + FEMTestSolver.m_NodePosRot_z[(i+1)*2]) * 0.5f;

			SimpleSocket.Init(FEMTestSolver, 0.1f, 0.09f, Anchor, i*2, i*2+1, (i+1)*2, (i+1)*2+1);

			ballsockets.push_back(SimpleSocket);
		}



		// 3rd
		for (i = (NumLinks+1)*2; i < NumLinks*3; ++i)
		{
			Anchor.x = (FEMTestSolver.m_NodePosRot_x[i*2] + FEMTestSolver.m_NodePosRot_x[(i+1)*2]) * 0.5f;
			Anchor.y = (FEMTestSolver.m_NodePosRot_y[i*2] + FEMTestSolver.m_NodePosRot_y[(i+1)*2]) * 0.5f;
			Anchor.z = (FEMTestSolver.m_NodePosRot_z[i*2] + FEMTestSolver.m_NodePosRot_z[(i+1)*2]) * 0.5f;

			SimpleSocket.Init(FEMTestSolver, 0.1f, 0.09f, Anchor, i*2, i*2+1, (i+1)*2, (i+1)*2+1);

			ballsockets.push_back(SimpleSocket);
		}

#endif
	}
	
	int sort_order;

	void Draw(const GLDrawer &gld, float dt)
	{
		Time += dt;

		if (!gbFreeze)
		{
			float solver_dt = 0.016f;

			FEMTestSolver.m_NumJoints = 0;

#if (DEMO_FEMCUBE == 1)
			//std::random_shuffle(tetrajoints.begin(), tetrajoints.end());


			std::vector<FEMJoint, GlobalAllocator<FEMJoint>>::iterator itJointEnd = tetrajoints.end(), itJoint;
			for (itJoint = tetrajoints.begin(); itJoint != itJointEnd; ++itJoint)
			{
				itJoint->UpdateCopy(solver_dt, FEMTestSolver);
			}


			std::vector<PlaneConstraint, GlobalAllocator<PlaneConstraint>>::iterator itPlaneEnd = planejoints.end(), itPlane;
			for (itPlane = planejoints.begin(); itPlane != itPlaneEnd; ++itPlane)
			{
				itPlane->UpdateCopy(solver_dt, FEMTestSolver);
			}


			std::vector<BallSocket, GlobalAllocator<BallSocket>>::iterator itBallSocketEnd = ballsockets.end(), itBallSocket;
			for (itBallSocket = ballsockets.begin(); itBallSocket != itBallSocketEnd; ++itBallSocket)
			{
				itBallSocket->UpdateCopy(solver_dt, FEMTestSolver);
			}

			std::vector<BallSocket_FEM, GlobalAllocator<BallSocket_FEM>>::iterator itBallSocketFEMEnd = ballsockets_fem.end(), itBallSocketFEM;
			for (itBallSocketFEM = ballsockets_fem.begin(); itBallSocketFEM != itBallSocketFEMEnd; ++itBallSocketFEM)
			{
				itBallSocketFEM->UpdateCopy(solver_dt, FEMTestSolver);
			}
#else
/*
			if (sort_order)
			{
				std::sort(ballsockets.begin(), ballsockets.end(), std::less<BallSocket>());
				sort_order = 0;
			} else
			{
				std::sort(ballsockets.begin(), ballsockets.end(), std::greater<BallSocket>());
				sort_order = 1;
			}
*/

			std::vector<BallSocket, GlobalAllocator<BallSocket>>::iterator itJointEnd = ballsockets.end(), itJoint;
			for (itJoint = ballsockets.begin(); itJoint != itJointEnd; ++itJoint)
			{
				itJoint->UpdateCopy(solver_dt, FEMTestSolver);
			}
#endif

			FEMTestSolver.Solve(solver_dt);


#if (DEMO_FEMCUBE == 1)

			itJointEnd = tetrajoints.end();
			for (itJoint = tetrajoints.begin(); itJoint != itJointEnd; ++itJoint)
			{
				itJoint->FetchLambdas(FEMTestSolver);
			}

			itPlaneEnd = planejoints.end();
			for (itPlane = planejoints.begin(); itPlane != itPlaneEnd; ++itPlane)
			{
				itPlane->FetchLambdas(FEMTestSolver);
			}


			itBallSocketEnd = ballsockets.end();
			for (itBallSocket = ballsockets.begin(); itBallSocket != itBallSocketEnd; ++itBallSocket)
			{
				itBallSocket->FetchLambdas(FEMTestSolver);
			}

			itBallSocketFEMEnd = ballsockets_fem.end();
			for (itBallSocketFEM = ballsockets_fem.begin(); itBallSocketFEM != itBallSocketFEMEnd; ++itBallSocketFEM)
			{
				itBallSocketFEM->FetchLambdas(FEMTestSolver);
			}

#else
			itJointEnd = ballsockets.end();
			for (itJoint = ballsockets.begin(); itJoint != itJointEnd; ++itJoint)
			{
				itJoint->FetchLambdas(FEMTestSolver);
			}

#endif
			UpdateFEM = false;
		}

		gld.DrawBasis(Vec3(-2.0f, -1.0f, 1.0f));
		gld.SetColor(1.0f, 1.0f, 0.0f, 0.3f);

		int cnt = 0;
		float slowTime = Time * 0.0002f;								// Time "slowed down"

		Vec3 PlaneNormal;
		PlaneNormal.x = sinf(1.5f * slowTime);
		PlaneNormal.y = cosf(1.3f * slowTime);
		PlaneNormal.z = sinf(1.9f * slowTime) * cosf(1.7f * slowTime);

		PlaneNormal.Normalize();

		unsigned jointCnt = 0;

		std::vector<FEMTetrahedron, GlobalAllocator<FEMTetrahedron>>::iterator itTetraEnd = FEMCube.FEMTetras.end();
		for (std::vector<FEMTetrahedron, GlobalAllocator<FEMTetrahedron>>::iterator itTetra = FEMCube.FEMTetras.begin(); itTetra != itTetraEnd; ++itTetra)
		{
#if (RENDER_OBJECT == 1)
			const Vec3	&v1 = FEMCube.FEMNodes[itTetra->ind1],
						&v2 = FEMCube.FEMNodes[itTetra->ind2],
						&v3 = FEMCube.FEMNodes[itTetra->ind3],
						&v4 = FEMCube.FEMNodes[itTetra->ind4];
#else
			Vec3	v1 = FEMTestSolver.GetPosition(itTetra->ind1),
					v2 = FEMTestSolver.GetPosition(itTetra->ind2),
					v3 = FEMTestSolver.GetPosition(itTetra->ind3),
					v4 = FEMTestSolver.GetPosition(itTetra->ind4);
#endif

			// Check if all vertices is in positive halfspace of [PlaneNormal, 0]
// 			if ((PlaneNormal.Dot(v1) < 0.0f) || (PlaneNormal.Dot(v2) < 0.0f) ||
// 				(PlaneNormal.Dot(v3) < 0.0f) || (PlaneNormal.Dot(v4) < 0.0f))
// 			{
// 				continue;
// 			}

#if (COLORING_STRAIN == 1)
			float FEstrain = tetrajoints[jointCnt].strain * 4.0f;

			gld.SetColor(0.7f + FEstrain, 0.3f - FEstrain * 0.5f, 0.3f - FEstrain * 0.5f, 1.0f);
#else
			float lambda[6];
			float lambdaRenderScale = 0.001f;
			for (int i = 0; i < 6; ++i)
			{
				lambda[i] = 1.0f - lambdaRenderScale * fabsf(tetrajoints[jointCnt].m_lambda0[i]);
			}

			if (!LambdaRender)
			{
				// Linear
				gld.SetColor(lambda[0], lambda[1], lambda[2], 1.0f);
			}
			else
			{
				// Angular
				gld.SetColor(lambda[3], lambda[4], lambda[5], 1.0f);
			}
#endif

			gld.DrawTriangle(v3, v2, v1);
			gld.SetColor(1.0f, 0.0f, 0.0f, 1.0f);
			gld.DrawTriangle(v4, v1, v2);
			gld.SetColor(0.0f, 1.0f, 0.0f, 1.0f);
			gld.DrawTriangle(v4, v2, v3);
			gld.SetColor(0.0f, 0.0f, 1.0f, 1.0f);
			gld.DrawTriangle(v4, v3, v1);

			gld.SetColor(0.0f, 0.0f, 0.0f, 1.0f);
			gld.DrawLine(v1, v2);
			gld.DrawLine(v1, v3);
			gld.DrawLine(v1, v4);
			gld.DrawLine(v2, v3);
			gld.DrawLine(v3, v4);
			gld.DrawLine(v4, v2);

			++jointCnt;
		}

		//FEMTestSolver.m_NodeF[(8*2)*3 + 2]
//		int idx = FEMTestSolver.NumNodes - 8*2 - 1; +Y
//		int idx = FEMTestSolver.NumNodes - 8*2 - 2; +Z
//		int idx = FEMTestSolver.NumNodes - 8*2 - 5; -Z
//		int idx = FEMTestSolver.NumNodes - 8*2 - 6; -Y
/*
		int idx = 48;
		gld.SetColor(1.0f, 0.0f, 0.0f, 1.0f);
		gld.DrawPoint(Vec3(FEMTestSolver.m_NodePosRot[idx*4 + 0],FEMTestSolver.m_NodePosRot[idx*4 + 1],FEMTestSolver.m_NodePosRot[idx*4 + 2]), 50.0f);
//*/

		std::vector<PlaneConstraint, GlobalAllocator<PlaneConstraint>>::iterator itPlaneJEnd = planejoints.end(), itPlaneJ;
		for (itPlaneJ = planejoints.begin(); itPlaneJ != itPlaneJEnd; ++itPlaneJ)
		{
			if (!itPlaneJ->m_bActive) continue;

			Vec3 NodePos = FEMTestSolver.GetPosition(itPlaneJ->m_BodyIdx);
			Vec3 plLambda = itPlaneJ->m_PlaneNormal * itPlaneJ->m_lambda0[0];

			

//			float color = fabsf(itPlaneJ->m_Violation);
			float color = fabsf(FEMTestSolver.m_resid[itPlaneJ->m_StartIdx]) * 1000.0f;

			gld.SetColor(0.7f, color * 0.25f, color * 0.5f, 1.0f);
			gld.DrawLine(NodePos, NodePos+plLambda);
		}
		gld.SetColor(1.0f, 1.0f, 1.0f, 1.0f);


		Vec3 wsX, wsY, wsZ, Pos;
		for (unsigned int i = 0; i < FEMTestSolver.m_NumNodes - 1; ++i)
		{
			// We need linear nodes, sequenced by rotational ones [full rigid body]
			if ((!FEMTestSolver.m_IsRotational[i+1]) || FEMTestSolver.m_IsRotational[i]) continue;
			CMatrix3 Rotation;
			CQuaternion(FEMTestSolver.m_NodePosRot_w[i+1], FEMTestSolver.m_NodePosRot_x[i+1],
						FEMTestSolver.m_NodePosRot_y[i+1], FEMTestSolver.m_NodePosRot_z[i+1]).ToMatrix3(Rotation);
			Pos.x = FEMTestSolver.m_NodePosRot_x[i];
			Pos.y = FEMTestSolver.m_NodePosRot_y[i];
			Pos.z = FEMTestSolver.m_NodePosRot_z[i];

			// Get Basis
			wsX = Vec3(Rotation.mMatrix[0][0], Rotation.mMatrix[1][0], Rotation.mMatrix[2][0]);
			wsY = Vec3(Rotation.mMatrix[0][1], Rotation.mMatrix[1][1], Rotation.mMatrix[2][1]);
			wsZ = Vec3(Rotation.mMatrix[0][2], Rotation.mMatrix[1][2], Rotation.mMatrix[2][2]);

			gld.SetColor(1.0f, 0.0f, 0.0f, 1.0f);
			gld.DrawLine(Pos, Pos + wsX);

			gld.SetColor(0.0f, 1.0f, 0.0f, 1.0f);
			gld.DrawLine(Pos, Pos + wsY);

			gld.SetColor(0.0f, 0.0f, 1.0f, 1.0f);
			gld.DrawLine(Pos, Pos + wsZ);
		}

		gld.PrintText("Cube Subdivision Demo");
		gld.PrintText("w: %f, %f, %f",	FEMTestSolver.m_NodeVel_x[FEMTestSolver.m_NumNodes - 2],
										FEMTestSolver.m_NodeVel_y[FEMTestSolver.m_NumNodes - 2],
										FEMTestSolver.m_NodeVel_z[FEMTestSolver.m_NumNodes - 2] );

		gld.RenderText(-0.99f, 0.85f, "||Gradient||^2 = %f", FEMTestSolver.GetGradNormSq());
		gld.RenderText(-0.99f, 0.80f, "||Lambda||^2 = %f", FEMTestSolver.GetLambdaNormSq());
		gld.RenderText(-0.99f, 0.75f, "Lambda dot Grad = %f", FEMTestSolver.GetDotLambdaGrad());
		gld.RenderText(-0.99f, 0.70f, "Effective Iterations: %d", FEMTestSolver.GetEffectiveIterations());
	}

private:

	int CubeSegWidth, CubeSegHeight, CubeSegDepth;
	FEMObject FEMCube;

	float Time;
} demo;

HWND hMainWindow = NULL;

GLDrawer drawer;
CCamera camera;
unsigned char keys[256];				// Array for Key States

LRESULT CALLBACK WndProc(HWND, UINT, WPARAM, LPARAM);	// WndProc function prototype

const int windowX = 640, windowY = 480;
const float znear = 0.01f, zfar = 100.0f;
const float fov = 45.0f;
const char *szTitle = "Physics demo framework ver. 0.01";

bool gbFullScreen;
bool gbWireframe = false;
float gDTime;

// FPS timer ID
#define FPS_TIMER 1
// For correct FPS counting and printing
int debugFPSCount, exFPSCount;

bool ProcessKeys(void)
{
	if(keys[VK_ESCAPE])
		return false;

	if (keys[VK_TAB])
	{
		// To prevent several switches
		keys[VK_TAB] = false;
		gbWireframe = !gbWireframe;
	}

	// Camera Movement
	if(keys['W'])
	{
		if (!keys[VK_SHIFT])
			camera.MoveForward( gDTime / 200.0f);
		else
			camera.MoveForward( gDTime / 20.0f);
	}

	if(keys['S'])
	{
		if (!keys[VK_SHIFT])
			camera.MoveForward(-gDTime / 200.0f);
		else
			camera.MoveForward(-gDTime / 20.0f);
	}

	if(keys['A'])
	{
		if (!keys[VK_SHIFT])
			camera.Strafe(-gDTime / 200.0f);
		else
			camera.Strafe(-gDTime / 20.0f);
	}

	if (keys['D'])
	{
		if (!keys[VK_SHIFT])
			camera.Strafe( gDTime / 200.0f);
		else
			camera.Strafe( gDTime / 20.0f);
	}

	if (keys[VK_PRIOR])
	{
		if (!keys[VK_SHIFT])
			camera.MoveUp( gDTime / 200.0f);
		else
			camera.MoveUp( gDTime / 20.0f);
	}

	if (keys[VK_NEXT] )
	{
		if (!keys[VK_SHIFT])
			camera.MoveUp(-gDTime / 200.0f);
		else
			camera.MoveUp(-gDTime / 20.0f);
	}//*/

	if (keys[VK_SPACE])
	{
		keys[VK_SPACE] = false;
		UpdateFEM = true;
	}

	if (keys['F'])
	{
		keys['F'] = false;

// 		demo.FEMTestSolver.m_NodeF[(demo.FEMTestSolver.NumNodes - 2)*3 + 0] += 100.0f;
		demo.FEMTestSolver.m_NodeF_y[demo.FEMTestSolver.m_NumNodes - 2] += 20000.0f;
//		demo.FEMTestSolver.m_NodeF[(demo.FEMTestSolver.NumNodes - 2)*3 + 2] += 200.0f;
	}

	if (keys['G'])
	{
		keys['G'] = false;

// 		demo.FEMTestSolver.m_NodeF[(demo.FEMTestSolver.NumNodes - 2)*3 + 0] -= 100.0f;
		demo.FEMTestSolver.m_NodeF_y[demo.FEMTestSolver.m_NumNodes - 2] -= 20000.0f;
//		demo.FEMTestSolver.m_NodeF[(demo.FEMTestSolver.NumNodes - 2)*3 + 2] -= 200.0f;
	}

	if (keys['T'])
	{
		keys['T'] = false;

		// FAILED TWIST [Need to co-rotate twist vectors for correct twist]
		float TwistMagn = 40000.0f;
		//		int idx = FEMTestSolver.NumNodes - 8*2 - 1; +Y
		//		int idx = FEMTestSolver.NumNodes - 8*2 - 2; +Z
		//		int idx = FEMTestSolver.NumNodes - 8*2 - 5; -Z
		//		int idx = FEMTestSolver.NumNodes - 8*2 - 6; -Y
		demo.FEMTestSolver.m_NodeF_y[demo.FEMTestSolver.m_NumNodes - 8*2 - 1] += TwistMagn;
		demo.FEMTestSolver.m_NodeF_z[demo.FEMTestSolver.m_NumNodes - 8*2 - 2] += TwistMagn;
		demo.FEMTestSolver.m_NodeF_z[demo.FEMTestSolver.m_NumNodes - 8*2 - 5] -= TwistMagn;
		demo.FEMTestSolver.m_NodeF_y[demo.FEMTestSolver.m_NumNodes - 8*2 - 6] -= TwistMagn;
	}

	if (keys['K'])
	{
		keys['K'] = false;

		if (demo.LambdaRender)
			demo.LambdaRender = 0;
		else
			demo.LambdaRender = 1;
	}

	if (keys['O'])
	{
		keys['O'] = false;

		demo.FEMTestSolver.CalcSylvesterCriterion();
	}

	if (keys['Q'])
	{
		keys['Q'] = false;

		gbFreeze = !gbFreeze;
	}

	if (keys['P'])
	{
		keys['P'] = false;

		std::vector<FEMJoint, GlobalAllocator<FEMJoint>>::iterator itJointEnd = demo.tetrajoints.end(), itJoint;
		for (itJoint = demo.tetrajoints.begin(); itJoint != itJointEnd; ++itJoint)
		{
			float NewYoung = itJoint->GetYoung() + 10000.0f;
			float NewPoisson = itJoint->GetPoisson();// + 0.1f;
			itJoint->SetYoungPoisson(NewYoung, (NewPoisson < 0.45f) ? NewPoisson : 0.45f);
		}
	}

	if (keys['L'])
	{
		keys['L'] = false;

		std::vector<FEMJoint, GlobalAllocator<FEMJoint>>::iterator itJointEnd = demo.tetrajoints.end(), itJoint;
		for (itJoint = demo.tetrajoints.begin(); itJoint != itJointEnd; ++itJoint)
		{
			float NewYoung = itJoint->GetYoung() - 10000.0f;
			float NewPoisson = itJoint->GetPoisson();// - 0.1f;
			itJoint->SetYoungPoisson((NewYoung > 1.0f) ? NewYoung : 1.0f, (NewPoisson > -0.45f) ? NewPoisson : -0.45f);
		}
	}

	return true;
}

// WndProc Body
/* This function better explains in NeHe's first lesson */
LRESULT CALLBACK WndProc(HWND hWnd, UINT uMsg,			// Handle && Message for Window
						 WPARAM wParam, LPARAM lParam)	// Message parameters
{
	switch (uMsg)
	{
		case WM_SYSCOMMAND:
		{
			switch (wParam)
			{
				case SC_SCREENSAVE:		// Don't let Screen Saver Start
				case SC_MONITORPOWER:	// Don't let Monitor enter in PowerSafe mode
				return 0;
			}
			break;
		}
		case WM_SIZE:	// Resizing Window
		{
			drawer.SetViewport(LOWORD(lParam), HIWORD(lParam), fov, znear, zfar);
			return 0;
		}
		case WM_CLOSE:	// Closing Window
		{
			PostQuitMessage(0);
			return 0;
		}
		case WM_KEYDOWN:	// Key Pressed
		{
			keys[wParam] = true;
			return 0;
		}
		case WM_KEYUP:		// Key Pressed
		{
			keys[wParam] = false;
			return 0;
		}
	}
	
	// Pass unhandled messages to DefWindowProc
	return DefWindowProc(hWnd,uMsg,wParam,lParam);
}

// Function to calculate FPS
void __stdcall TimerProc(struct HWND__ *,unsigned int,unsigned int,unsigned long)
{
	char TempFPS[256];
	sprintf(TempFPS, "%s - [%04d] fps", szTitle, debugFPSCount);
	SetWindowText(hMainWindow, TempFPS);
	exFPSCount = debugFPSCount;
	debugFPSCount = 0;
}

void Render(demoscene &demo, float dt)
{
	drawer.BeginDraw();
	drawer.RenderFPS(exFPSCount);
	drawer.BeginScene(camera, gbWireframe);
	demo.Draw(drawer, dt);
	drawer.EndScene(camera, gbWireframe);
	drawer.EndDraw();
}

// Main Function
int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance,
				   LPSTR lpCmdLine, int nShowCmd)
{
	_CrtSetDbgFlag ( _CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF );

	srand(GetTickCount());

// 	gLog2.SetLevel(LOGLEV_DEBUG);
// 	gLog2.Initialize("runlog.log");
// 	gLog2.LogDebug("test");

	// Ask user if he want to start fullscreen
	if (MessageBox(NULL, "FullScreen [recommended] ?", "TEngine Question", MB_YESNO | MB_ICONQUESTION) == IDYES)
		gbFullScreen = true;
	else
		gbFullScreen = false;
	
	WNDCLASS wc;
	
	// Setting Window Class
	wc.style         = CS_HREDRAW | CS_VREDRAW | CS_OWNDC;	// Rendraw on move and own DC for Window
	wc.lpfnWndProc   = (WNDPROC)WndProc;					// WndProc function handles messages
	wc.cbClsExtra    = 0;									// 
	wc.cbWndExtra    = 0;									// No extra Window data
	wc.hInstance     = hInstance;							// Instance
	wc.hIcon         = LoadIcon(NULL, IDI_WINLOGO);			// Load the Default Icon for Window
	wc.hCursor       = LoadCursor(NULL, IDC_ARROW);			// Load Arrow Pointer for Mouse
	wc.hbrBackground = NULL;								// No Background
	wc.lpszMenuName	 = NULL;								// No Menu
	wc.lpszClassName = "Tutorial Engine";					// Setting Class Name

	if (!RegisterClass(&wc))
	{
		MessageBox(NULL, "Failed To Register The Window Class.", "TEngine Error", MB_OK | MB_ICONEXCLAMATION);
		return 0;
	}
	
	RECT		WindowRect;		// Window rectangle

	// Setting Window rectangle
	WindowRect.left		= 0;
	WindowRect.right	= (long) windowX;
	WindowRect.top		= 0;
	WindowRect.bottom	= (long) windowY;

	DWORD dwStyle;
	// Setting Window Style
	if (gbFullScreen)
		dwStyle = WS_POPUP;
	else
		dwStyle = WS_CAPTION;	// We don't need our window to resize

	// Adjust Window to requested size
	AdjustWindowRect(&WindowRect, dwStyle, FALSE);

	long x = gbFullScreen ? 0 : (GetSystemMetrics(SM_CXSCREEN) - (WindowRect.right - WindowRect.left)) / 2;
	long y = gbFullScreen ? 0 : (GetSystemMetrics(SM_CYSCREEN) - (WindowRect.bottom - WindowRect.top)) / 2;

	HWND hwnd;
	// Trying to Create Window
	if (!(hwnd = CreateWindowEx(WS_EX_APPWINDOW,
							   "Tutorial Engine",
							   szTitle,
							   dwStyle,
							   x,
							   y,
							   WindowRect.right - WindowRect.left,
							   WindowRect.bottom - WindowRect.top,
							   NULL,
							   NULL,
							   hInstance,
							   NULL)))	
	{
		MessageBox(NULL, "Can't create window.", "TEngine Error", MB_OK | MB_ICONEXCLAMATION);
		return 0;
	}

	hMainWindow = hwnd;

	// Toggle Showing Cursor
	if (gbFullScreen)
		ShowCursor(false);
	else 
		ShowCursor(true);
	
	std::string error;

	if(!drawer.Init(hwnd, hInstance, gbFullScreen, windowX, windowY, error))
	{
		MessageBox(NULL, error.c_str(), "TEngine Error", MB_OK | MB_ICONEXCLAMATION);
		return 0;
	}
	
	ShowWindow(hwnd, SW_SHOW);	// Show Window
	SetForegroundWindow(hwnd);	// Higher priority
	SetFocus(hwnd);			// Set Focus to Window

	drawer.SetViewport(windowX, windowY, fov, znear, zfar);
	
	int middleX = GetSystemMetrics(SM_CXSCREEN) >> 1;
	int middleY = GetSystemMetrics(SM_CYSCREEN) >> 1;

	camera.SetScreenCenterCoords(middleX, middleY);
	camera.Setup(Vec3(0.0f, 5.0f, 15.0f), Vec3(0.0f, 0.0f, 0.0f));
	
	SetWindowText(hwnd, szTitle);
	
	// Setting Cursor Position in centre Window
	SetCursorPos(middleX, middleY);

	// Setting Timer for FPS counting
	SetTimer(hwnd, FPS_TIMER, 1000, TimerProc);

	DWORD PrgBegin;

	// TimeSync
	PrgBegin = GetTickCount();

	int ElapsedTime = 0;					// Simple Time Syncronization
	
	MSG msg; // Message structure
	bool bLoop = true;

	demo.Init();

	while (bLoop)
	{
		if (PeekMessage(&msg, NULL, 0, 0, PM_REMOVE))
		{
			// Handles some more messages
			switch (msg.message) 
			{
			    case WM_QUIT:
					 bLoop = true;
				     break;
			    default:
					 TranslateMessage(&msg);
					 DispatchMessage(&msg);
			         break;
			};
			
		}
		else
		{
			debugFPSCount++;							// For FPS counting
		
			int prevTime = ElapsedTime;
			ElapsedTime = GetTickCount() - PrgBegin;	// Some Time Synchronization

			gDTime = (float) ElapsedTime - prevTime;

			POINT mousemPos;
			GetCursorPos(&mousemPos);
			camera.OnMouse(mousemPos.x, mousemPos.y);
			camera.GetSavedMouseCoords(mousemPos.x, mousemPos.y);
			SetCursorPos(mousemPos.x, mousemPos.y);

			// We should check Esc key here, not in ProcessKeys function
			if (!ProcessKeys())
				bLoop = false;
			Render(demo, gDTime);								// Render Current Scene
		}
	}
	
	drawer.Destroy();		// Destroy Scene

	return (int)msg.wParam;
}
